Angular speed derivation device and angular speed derivation method for deriving angular speed based on output value of triaxial gyro sensor

ABSTRACT

A first converter converts an initial attitude in an Euler angle representation into an initial attitude represented by quaternion. An updating unit updates an attitude represented by quaternion by defining the initial attitude represented by quaternion as an initial value, successively substituting output values of the triaxial gyro sensor. A second converter converts the attitude represented by quaternion into an attitude in the Euler angle representation. An angular speed derivation unit derives an angular speed based on a time-dependent change in the attitude in the Euler angle representation. A controller adjusts a period of time for derivation by an initial attitude derivation unit based on a variance value of the output value of the triaxial acceleration sensor or a variance value of the output value of the triaxial gyro sensor, when the speed is lower than a threshold value.

CROSS-REFERENCE TO RELATED APPLICATION

This application is based upon and claims the benefit of priority fromthe prior Japanese Patent Application No. 2017-199791, filed on Oct. 13,2017, and Japanese Patent Application No. 2017-199792, filed on Oct. 13,2017, the entire contents of which are incorporated herein by reference.

BACKGROUND 1. Field

The present invention relates to an angular speed derivation technologyand, more particularly, to an angular speed derivation device and anangular speed derivation method for deriving an angular speed based onan output value of a triaxial gyro sensor.

2. Description of the Related Art

Means provided in a vehicle-mounted navigation device to detect acurrent position of an automobile are categorized into autonomous(dead-reckoning) navigation using a distance sensor and an angularsensor and a satellite navigation using a global navigation satellitesystem (GNSS) receiver for receiving radio waves from a satellite.

In the former means, the sensitivity of a gyro sensor drops when thevehicle is inclined as compared to to when the vehicle is aligned with ahorizontal axis. For this reason, an angle of inclination of a slope,i.e., a pitch angle, is derived based on outputs from a triaxialacceleration sensor and a speed sensor and an output of the gyro sensoris corrected by the pitch angle (see, for example, patent document 1).

[patent document 1] JP-A-Hei-9-42979

When a monoaxial gyro sensor is used, the sensitivity error caused bythe roll and pitch of the gyro sensor may deteriorate the accuracy ofdetecting the position. To resolve the issue, it is effective to use atriaxial gyro sensor. The absolute attitude angle can be calculated byintegrating the angular speed calculated based on the gyro sensor.Accordingly, the yaw angle that allows for the inclination of the sensorcan be derived. However, errors included in the angular speed of therespective axes are also integrated into the absolute attitude angle. Itis therefore desired to inhibit the accuracy of detecting the angularspeed from becoming poor when a triaxial gyro sensor is used.

SUMMARY

An angular speed derivation device according to an embodiment can beinstalled in a mobile object, and includes: an initial attitudederivation unit that derives an initial attitude in an Euler anglerepresentation based on an output value of a triaxial accelerationsensor; a first converter that converts the initial attitude in theEuler angle representation derived by the initial attitude derivationunit into an initial attitude represented by quaternion; an updatingunit that updates an attitude represented by quaternion by defining theinitial attitude represented by quaternion and derived from conversionin the first converter as an initial value, and repeatedly solving adifferential equation of the attitude represented by quaternion bysuccessively substituting output values of the triaxial gyro sensor intothe differential equation; a second converter that converts the attituderepresented by quaternion and updated by the updating unit into anattitude in the Euler angle representation; an angular speed derivationunit that derives an angular speed based on a time-dependent change inthe attitude in the Euler angle representation derived from conversionin the second converter; an acquisition unit that acquires a speed ofthe mobile object; and a controller that adjusts a period of time forderivation by the initial attitude derivation unit based on a variancevalue of the output value of the triaxial acceleration sensor or avariance value of the output value of the triaxial gyro sensor, when thespeed acquired by the acquisition unit is lower than a threshold value.

Another embodiment also relates to an angular speed derivation device.The device can be installed in a mobile object and includes: a firstconverter that converts an initial attitude in an Euler anglerepresentation derived based on an output value of a triaxialacceleration sensor into an initial attitude represented by quaternion;an updating unit that updates the attitude represented by quaternion bydefining the initial attitude represented by quaternion and derived fromconversion in the first converter as an initial value, and repeatedlysolving a differential equation of the attitude represented byquaternion by successively substituting output values of a triaxial gyrosensor into the differential equation; a second converter that convertsthe attitude represented by quaternion and updated by the updating unitinto an attitude in the Euler angle representation; an angular speedderivation unit that derives an angular speed based on a time-dependentchange in the attitude in the Euler angle representation derived fromconversion in the second converter; and an output unit that outputs theangular speed derived by the angular speed derivation unit. The outputunit outputs an angular speed derived based on an output value of amonoaxial gyro sensor instead of the angular speed derived by theangular speed derivation unit, when a value of integral of absolutevalues of the angular speed derived by the angular speed derivation unitis equal to or larger than a threshold value.

Still another embodiment also relates to an angular speed derivationdevice. The device can be installed in a mobile object and includes: afirst converter that converts an initial attitude in an Euler anglerepresentation derived based on an output value of a triaxialacceleration sensor into an initial attitude represented by quaternion;an updating unit that updates the attitude represented by quaternion bydefining the initial attitude represented by quaternion and derived fromconversion in the first converter as an initial value, and repeatedlysolving a differential equation of the attitude represented byquaternion by successively substituting output values of a triaxial gyrosensor into the differential equation; a second converter that convertsthe attitude represented by quaternion and updated by the updating unitinto an attitude in the Euler angle representation; an angular speedderivation unit that derives an angular speed based on a time-dependentchange in the attitude in the Euler angle representation derived fromconversion in the second converter; an output unit that outputs theangular speed derived by the angular speed derivation unit; and anacquisition unit that acquires a temperature. The output unit outputs anangular speed derived based on an output value of a monoaxial gyrosensor instead of the angular speed derived by the angular speedderivation unit, when a variation in the temperature acquired by theacquisition unit is equal to or larger than a threshold value.

Still another embodiment relates to an angular speed derivation method.The method is adapted for an angular speed derivation device that can beinstalled in a mobile object and includes: deriving an initial attitudein an Euler angle representation based on an output value of a triaxialacceleration sensor; converting the initial attitude in the Euler anglerepresentation derived into an initial attitude represented byquaternion; updating the attitude represented by quaternion by definingthe initial attitude represented by quaternion derived from conversionas an initial value, and repeatedly solving a differential equation ofthe attitude represented by quaternion by successively substitutingoutput values of a triaxial gyro sensor into the differential equation;converting the attitude represented by quaternion as updated into anattitude in the Euler angle representation; deriving an angular speedbased on a time-dependent change in the attitude in the Euler anglerepresentation derived from conversion; acquiring a speed of the mobileobject; and adjusting a period of time for derivation of the initialattitude in the Euler angle representation based on a variance value ofthe output value of the triaxial acceleration sensor or a variance valueof the output value of the triaxial gyro sensor, when the speed acquiredis lower than a threshold value.

Still another embodiment relates to an angular speed derivation method.The method is adapted for an angular speed derivation device that can beinstalled in a mobile object and includes: converting an initialattitude in an Euler angle representation derived based on an outputvalue of a triaxial acceleration sensor into an initial attituderepresented by quaternion; updating the attitude represented byquaternion by defining the initial attitude represented by quaternionderived from conversion as an initial value, and repeatedly solving adifferential equation of the attitude represented by quaternion bysuccessively substituting output values of a triaxial gyro sensor intothe differential equation; converting the attitude represented byquaternion as updated into an attitude in the Euler anglerepresentation; deriving an angular speed based on a time-dependentchange in the attitude in the Euler angle representation derived fromconversion; and outputting the angular speed derived. The outputtingoutputs an angular speed derived based on an output value of a monoaxialgyro sensor, when a value of integral of absolute values of the angularspeed derived is equal to or larger than a threshold value.

Still another embodiment relates to an angular speed derivation method.An angular speed derivation method adapted for an angular speedderivation device that can be installed in a mobile object, comprising:converting an initial attitude in an Euler angle representation derivedbased on an output value of a triaxial acceleration sensor into aninitial attitude represented by quaternion; updating the attituderepresented by quaternion by defining the initial attitude representedby quaternion derived from conversion as an initial value, andrepeatedly solving a differential equation of the attitude representedby quaternion by successively substituting output values of a triaxialgyro sensor into the differential equation; converting the attituderepresented by quaternion as updated into an attitude in the Euler anglerepresentation; deriving an angular speed based on a time-dependentchange in the attitude in the Euler angle representation derived fromconversion; outputting the angular speed derived; and acquiring atemperature. The outputting outputs an angular speed derived based on anoutput value of a monoaxial gyro sensor, when a variation in thetemperature acquired is equal to or larger than a threshold value.

Optional combinations of the aforementioned constituting elements, andimplementations of the embodiments in the form of methods, apparatuses,systems, recording mediums, and computer programs may also be practicedas additional modes of the embodiments.

BRIEF DESCRIPTION OF THE DRAWINGS

Embodiments will now be described by way of examples only, withreference to the accompanying drawings which are meant to be exemplary,not limiting and wherein like elements are numbered alike in severalFigures in which:

FIG. 1 shows a configuration of an angular speed derivation deviceaccording to Embodiment 1;

FIG. 2 shows a coordinate system according to Embodiment 1;

FIG. 3 is a flowchart showing steps for adjustment of a period of timefor derivation by the angular speed derivation device of FIG. 1 ;

FIG. 4 shows a configuration of the angular speed derivation deviceaccording to Embodiment 2;

FIG. 5 shows a summary of the process in the pitch angle derivation unitof FIG. 4 ;

FIG. 6 is a flowchart showing steps for switching by the angular speedderivation device of FIG. 4 ;

FIG. 7 shows a configuration of an angular speed derivation deviceaccording to Embodiment 3;

FIG. 8 is a flowchart showing steps for switching by the angular speedderivation device of FIG. 7 ;

FIG. 9 shows a configuration of an angular speed derivation deviceaccording to Embodiment 4; and

FIG. 10 is a flowchart showing steps for switching by the angular speedderivation device of FIG. 9 .

DETAILED DESCRIPTION

The invention will now be described by reference to the preferredembodiments. This does not intend to limit the scope of the presentinvention, but to exemplify the invention.

A summary of the present invention will be given before describing theinvention in specific detail. An embodiment relates to an angular speedderivation device mounted in a vehicle, etc. and configured to derive anangular speed by using a triaxial gyro sensor. An angular speedderivation device updates an initial attitude derived by referring to atriaxial acceleration sensor with an output value of the triaxial gyrosensor and derives the angular speed from a difference between theupdated attitude and previous attitude. An attitude angle is a synthesisof an inclination created when the triaxial gyro sensor is installed inthe vehicle etc. and an inclination caused by a vehicle condition suchas a slope or a bank. The attitude angle is ultimately defined relativeto the three axes (vertical/sagittal/transversal) with reference to thehorizontal ground level. The angular speed output from the triaxial gyrosensor is represented in a coordinate system defined with reference tothe sensor itself and is not represented in a coordinate system definedwith reference to the horizontal ground level. It is therefore necessaryto convert the angular speed output from the triaxial gyro sensor to fitthe coordinate system defined with reference to the horizontal groundlevel.

Thus, the attitude is derived by deriving the initial attitude from theoutput value from the triaxial acceleration sensor and updating theinitial attitude with the angular speed output from the triaxial gyrosensor. The attitude can be represented in three methods including Eulerangle, direction cosine matrix, and quaternion. For initial attitude andattitude, Euler angle is used. For an updating process, quaternion isused. In deriving an attitude angle based on the output value of thetriaxial acceleration sensor, errors in the absolute attitude angle maybe collected in deriving the initial attitude and in updating theattitude. Collection of errors lowers the accuracy of deriving the yawangular speed. This embodiment is directed to the purpose of reducing anerror in deriving the initial attitude and thereby inhibiting theaccuracy of deriving the attitude angle and angular speed from becomingpoor.

When a vehicle is at a stop, the translational acceleration andcentripetal acceleration of the vehicle is 0 so that the initialattitude can be derived accurately. In the presence of an impact fromvibration of a man-made structure like a bridge or vibration of thevehicle associated with a person getting on and off the vehicle, anassociated acceleration component causes the accuracy of deriving theinitial attitude to become poor. To address this, the angular speedderivation device according to the embodiment adjusts a period of timeconsumed for derivation of the initial attitude in accordance with thedegree of variation in the attitude when the speed of the vehicle islower than a threshold value.

FIG. 1 shows a configuration of an angular speed derivation device 100according to Embodiment 1. The angular speed derivation device 100includes a triaxial acceleration sensor 10, an initial attitudederivation unit 12, a first converter 14, a triaxial gyro sensor 16, anupdating unit 18, a second converter 20, an angular speed derivationunit 22, an output unit 24, a speed sensor 26, and a controller 28.Further, the controller 28 includes a substantial stop determinationunit 50, a variance value acquisition unit 52, a normalization unit 54,and an adjuster 56. The angular speed derivation device 100 can beinstalled in a vehicle (not shown).

The triaxial acceleration sensor 10 measures an acceleration in thethree axes respectively. FIG. 2 will be used to explain the three axes.FIG. 2 shows a coordinate system according to Embodiment 1. A quadraturecoordinate system formed by three axes including an x-axis, y-axis, andz-axis is defined. The x-axis is oriented in a direction of travel of avehicle 200, the z-axis is aligned with a downward normal direction fromthe floor level of of vehicle 200, and the y-axis is oriented in adirection perpendicular to the x-axis and the z-axis. The triaxialacceleration sensor 10 acquires an acceleration a_(x) in the x-axisdirection, an acceleration a_(y) in the y-axis direction, and anacceleration a_(z) in the z-axis direction by making measurements.Reference is made back to FIG. 1 . The triaxial acceleration sensor 10outputs these to the initial attitude derivation unit 12 as an outputvalue of the triaxial acceleration sensor 10.

The initial attitude derivation unit 12 receives the output value of thetriaxial acceleration sensor 10. The initial attitude derivation unit 12derives the initial attitude in the Euler angle representation based onthe output value of the triaxial acceleration sensor 10. In the Eulerangle representation, the angle of rotation around the x-axis isreferred to as a roll angle φ, the angle of rotation around the y-axisis referred to as a pitch angle θ, and the angle of rotation around thez-axis is referred to as a yaw angle ψ. ψ and θ of the initial attitudeare denoted as follows.

$\begin{matrix}{{\phi = {\tan^{- 1}\left( \frac{a_{y}}{a_{z}} \right)}}{\phi = {\tan^{- 1}\left( \frac{a_{x}}{\sqrt{a_{y}^{2} + a_{z}^{2}}} \right)}}} & (1)\end{matrix}$

The initial value of ψ may be an arbitrary value and is set to, forexample, “0”.

The first converter 14 converts the initial attitude in the Euler anglerepresentation derived by the initial attitude derivation unit 12 intothe initial attitude represented by a direction cosine matrix, and then,converts the initial attitude represented y a direction cosine matrixinto the initial attitude represented by quaternion. The initialattitude in the Euler angle representation is converted into a directioncosine matrix E as follows.

$\begin{matrix}\begin{matrix}{E = {{\begin{bmatrix}1 & 0 & 0 \\0 & {\cos\;\phi} & {\sin\;\phi} \\0 & {{- \sin}\;\phi} & {\cos\;\phi}\end{bmatrix}\begin{bmatrix}{\cos\;\theta} & 0 & {{- \sin}\;\theta} \\0 & 1 & 0 \\{\sin\;\theta} & 0 & {\cos\;\theta}\end{bmatrix}}\begin{bmatrix}{\cos\;\psi} & {\sin\;\psi} & 0 \\{{- \sin}\;\psi} & {\cos\;\psi} & 0 \\0 & 0 & 1\end{bmatrix}}} \\{= \begin{bmatrix}{\cos\;{\theta cos\psi}} & {\cos\;{\theta sin\psi}} & {{- \sin}\;\theta} \\{{\sin\;\phi\;\sin\;{\theta cos\psi}} - {\cos\;{\phi sin}\;\psi}} & {{\sin\;{\phi sin}\;{\theta sin\psi}} + {\cos\;{\phi cos\psi}}} & {\sin\;{\phi cos\theta}} \\{{\cos\;{\phi sin}\;{\theta cos}\;\psi} + {\sin\;{\phi sin\psi}}} & {{\cos\;{\phi sin}\;{\theta sin}\;\psi} - {\sin\;{\phi cos\psi}}} & {\cos\;{\phi cos\theta}}\end{bmatrix}}\end{matrix} & (2)\end{matrix}$

The components of the direction cosine matrix E are given below.

$\begin{matrix}{E = \begin{bmatrix}E_{11} & E_{12} & E_{13} \\E_{21} & E_{22} & E_{23} \\E_{31} & E_{32} & E_{33}\end{bmatrix}} & (3)\end{matrix}$

A quaternion is defined by four components including unit vectors in thedirections of rotation and an angle of rotation. The initial attituderepresented by a direction cosine matrix E is converted into the initialattitude q₁, q₂, q₃, and q₄ in the quaternion representation as follows.

$\begin{matrix}{{q_{4} = {{\pm \frac{1}{2}}\sqrt{1 + E_{11} + E_{22} + E_{33}}}}{q_{1} = {\frac{1}{4q_{4}}\left( {E_{23} - E_{32}} \right)}}{q_{2} = {\frac{1}{4q_{4}}\left( {E_{31} - E_{13}} \right)}}{q_{3} = {\frac{1}{4q_{4}}\left( {E_{12} - E_{21}} \right)}}} & (4)\end{matrix}$

The first converter 14 outputs the initial attitude q₁, q₂, q₃, and q₄in the quaternion representation to the updating unit 18.

The triaxial gyro sensor 16 successively outputs angular speed vectorsω=[p q r]^(T) as the output value. The components p, q, and r are asshown in FIG. 2 . The triaxial gyro sensor 16 successively outputs theoutput value to the updating unit 18.

The updating unit 18 solves the following differential equation of thenext attitude in the quaternion representation.

$\begin{matrix}{{\frac{d}{dt}\begin{bmatrix}q_{1} \\q_{2} \\q_{3} \\q_{4}\end{bmatrix}} = {{\frac{1}{2}\begin{bmatrix}0 & r & {- q} & p \\{- r} & 0 & p & q \\q & {- p} & 0 & r \\{- p} & {- q} & {- r} & 0\end{bmatrix}}\begin{bmatrix}q_{1} \\q_{2} \\q_{3} \\q_{4}\end{bmatrix}}} & (5)\end{matrix}$

In the initial stage, the updating unit 18 substitutes the initialattitude q₁, q₂, q₃, and q₄ in the quaternion representation derivedfrom conversion in the first converter 14 into the right side of thedifferential equation. The updating unit 18 also substitutes thecomponents p, q, and r of the angular speed vector at that point of timeinto the right side of the differential equation. By solving thedifferential equation, the updating unit 18 derives the updated attitudeq₁, q₂, q₃, and q₄ in the quaternion representation.

Subsequently, the updating unit 18 substitutes the derived attitude q₁,q₂, q₃, and q₄ in the quaternion representation into the right side ofthe differential equation and also substitutes the components p, q, andr of the new angular speed vector into the right side of thedifferential equation. By solving the differential equation, theupdating unit 18 derives the attitude q₁, q₂, q₃, and q₄ in thequaternion representation again. In other words, the updating unit 18updates the attitude q₁, q₂, q₃, and q₄ in the quaternion representationby substituting the output values of the triaxial gyro sensor 16successively and repeatedly solving the differential equation of theattitude in the quaternion representation. The updating unit 18 outputsthe updated attitude q₁, q₂, q₃, and q₄ in the quaternion representationto the second converter 20.

The second converter 20 converts the attitude q₁, q₂, q₃, and q₄ in thequaternion representation updated by the updating unit 18 into theattitude represented by a direction cosine matrix E and then convertsthe attitude represented by the direction cosine matrix E into theattitude in the Euler angle representation. The updated attitude q₁, q₂,q₃, and q₄ in the quaternion representation is converted into theattitude represented by the direction cosine matrix E as follows.

$\begin{matrix}{E = \begin{bmatrix}{q_{1}^{2} - q_{2}^{2} - q_{3}^{2} + q_{4}^{2}} & {2\left( {{q_{1}q_{2}} + {q_{3}q_{4}}} \right)} & {2\left( {{q_{1}q_{3}} - {q_{2}q_{4}}} \right)} \\{2\left( {{q_{1}q_{2}} - {q_{3}q_{4}}} \right)} & {{- q_{1}^{2}} + q_{2}^{2} - q_{3}^{2} + q_{4}^{2}} & {2\left( {{q_{2}q_{3}} + {q_{1}q_{4}}} \right)} \\{2\left( {{q_{1}q_{3}} + {q_{2}q_{4}}} \right)} & {2\left( {{q_{2}q_{3}} - {q_{1}q_{4}}} \right)} & {{- q_{1}^{2}} - q_{2}^{2} + q_{3}^{2} + q_{4}^{2}}\end{bmatrix}} & (6)\end{matrix}$

The direction cosine matrix E is converted into the attitude in theEuler angle representation as follows.

$\begin{matrix}{{\phi = {\tan^{- 1}\frac{E_{23}}{E_{33}}}}{\theta = {\tan^{- 1}\frac{- E_{13}}{\sqrt{E_{23}^{2} + E_{33}^{2}}}}}{\psi = {\tan^{- 1}\frac{E_{12}}{E_{11}}}}} & (7)\end{matrix}$

Of the attitude in the Euler angle representation, the second converter20 outputs the yaw angle ψ to the angular speed derivation unit 22.

Of the attitude in the Euler angle representation derived fromconversion in the second converter 20, the angular speed derivation unit22 receives the yaw angle is. The angular speed derivation unit 22derives the angular speed based on the time-dependent change t [sec] inthe yaw angle ψ. Given, for example, that the yaw angle of time n ofinterest is denoted by and the yaw angle at time n−1 is denoted ψn−1,the angular speed is derived as (ψ_(n)−ψ_(n-1))/t. The output unit 24outputs the angular speed derived by the angular speed derivation unit22.

The speed sensor 26 is provided in the middle of a speed meter cablerotated in association with the rotation of the drive shaft and outputsa speed pulse signal in accordance with the rotation of the drive shaft.Further, the speed sensor 26 periodically detects the pulse count bycounting speed pulse signals output in association with the movement ofthe vehicle at predetermined periods. The pulse count is in proportionto the speed of the vehicle 200. It can therefore be said that the speedsensor 26 measures the speed of the vehicle 200. The speed sensor 26outputs the measured speed to the controller 28. The speed of thevehicle 200 may be acquired by a global navigation satellite system(GLASS) instead of the speed sensor 26.

The substantial stop determination unit 50 of the controller 28 receivesinformation on the speed from the speed sensor 26. When the informationindicates that the speed is lower than a threshold value (e.g., “0”)over a predetermined period of time, the substantial stop determinationunit 50 determines that the vehicle 200 is substantially at a stop. Whenthe substantial stop determination unit 50 determines that the vehicleis substantially at a stop, the substantial stop determination unit 50notifies the variance value acquisition unit 52 accordingly. Thethreshold value is set to a value with which it can be determined thatthe vehicle 200 is substantially at a stop. For example, a determinationthat the vehicle 200 is substantially at a stop can be made when thespeed sensor 26 cannot detect pulses for a predetermined period of time.

When a notification is received from the substantial stop determinationunit 50, the variance value acquisition unit 52 acquires a varianceVar_gyro of the output value of the triaxial gyro sensor 16. The periodover which a variance value is calculated is, for example, 10 seconds,and the sampling frequency is, for example, 10 [Hz]. This enablesdetection of a short-period change in the attitude of the vehicle 200.The variance value acquisition unit 52 outputs the variance valueVar_gyro to the normalization unit 54.

The normalization unit 54 receives the variance value Var_gyro from thevariance value acquisition unit 52. The normalization unit 54 maintainsa variance TypVar_gyro of the output value of the triaxial gyro sensor16 measured in advance through an experiment. The normalization unit 54normalizes the variance value Var_gyro by dividing it by the varianceTypVar_gyro. The normalization unit 54 outputs (Var_gyro/TypVar_gyro) tothe adjuster 56.

The adjuster 56 receives (Var_gyro/TypVar_gyro) from the normalizationunit 54. The adjuster 56 maintains an attitude angle derivationreference period Term_default determined in advance through anexperiment. A period of time shorter than the time during which thetemperature changes abruptly and longer than the time affected by whitenoise is set. For example, the period Term_default is set to threeseconds. The adjuster 56 multiplies the attitude angle derivationreference period Term_default by (Var_gyro/TypVar_gyro). The result ofmultiplication represents a period of time for derivation. In otherwords, the adjuster 56 adjusts the period of time for derivation suchthat the period of time consumed for derivation in the initial attitudederivation unit 12 is longer as the variance value of the output valueof the triaxial gyro sensor 16 becomes large. When(Var_gyro/TypVar_gyro) is 1 or smaller, the period of time forderivation is configured to be Term_default. When the period of time forderivation exceeds 10 seconds, the period of time for derivation isconfigured to be 10 seconds. The adjuster 56 sets the period of time forderivation in the initial attitude derivation unit 12. The initialattitude derivation unit 12 averages the initial attitude over theperiod of time for derivation thus set.

This is equivalent to determining the period of time for derivation byusing the normalized variance value of the angular speed. When thevariance value of the angular speed is large, the attitude of thevehicle 200 changes in a short period and a noise component isintroduced into the output value of the triaxial acceleration sensor 10.Therefore, the impact from a noise component is reduced by extending theperiod of time for derivation.

The variance value acquisition unit 52, the normalization unit 54, andthe adjuster 56 may perform the following process. When a notificationis received from the substantial stop determination unit 50, thevariance value acquisition unit 52 acquires the variance value Var_acclof the output value of the triaxial acceleration sensor 10. The periodof time over which a variance value is calculated is, for example, 10seconds, and the sampling frequency is, for example, 10 [Hz]. Thisenables detection of a change in the attitude of the vehicle 200. Thevariance value acquisition unit 52 outputs the variance value Var_acclto the normalization unit 54.

The normalization unit 54 receives the variance value Var_accl from thevariance value acquisition unit 52. The normalization unit 54 maintainsa variance TypVar_accl of the output value of the triaxial accelerationsensor 10 measured in advance through an experiment. The normalizationunit 54 normalizes the variance value Var_accl by dividing it by thevariance TypVar_accl. The normalization unit 54 outputs(Var_accl/TypVar_accl) to the adjuster 56.

The adjuster 56 receives (Var_accl/TypVar_accl) from the normalizationunit 54. The adjuster 56 maintains an attitude angle derivationreference period Term_default determined in advance through anexperiment. A period of time shorter than the time during which thetemperature changes abruptly and longer than the time affected by whitenoise is set. For example, the period of time Term_default is set tothree seconds. The adjuster 56 multiplies the attitude angle derivationreference period Term_default by (Var_accl/TypVar_accl). The result ofmultiplication represents a period of time for derivation. In otherwords, the adjuster 56 adjusts the period of time for derivation suchthat the period of time consumed for derivation in the initial attitudederivation unit 12 is longer as the variance value of the output valueof the triaxial acceleration sensor 10 increases. When(Var_accl/TypVar_accl) is 1 or smaller, the period of time forderivation is configured to be Term_default. When the period of time forderivation exceeds 10 seconds, the period of time for derivation isconfigured to be 10 seconds. The adjuster 56 sets the period of time forderivation in the initial attitude derivation unit 12. The initialattitude derivation unit 12 averages the initial attitude over theperiod of time for derivation thus set. This is equivalent todetermining the period of time for derivation by using the normalizedvariance value of the angular speed. That the variance value of theangular speed is large means that the attitude of the vehicle 200 haschanged. Therefore, the accuracy is improved by extending the period oftime for derivation.

The features are implemented in hardware such as a CPU, a memory, orother LSI's, of any computer and in software such as a program loadedinto a memory. The figure depicts functional blocks implemented by thecooperation of these elements. Therefore, it will be understood by thoseskilled in the art that the functional blocks may be implemented in avariety of manners by hardware only, software only, or by a combinationof hardware and software.

A description will be given of the operation of the angular speedderivation device 100 having the above configuration. FIG. 3 is aflowchart showing steps for adjustment of a period of time forderivation by the angular speed derivation device 100. When the speed issmaller than a threshold value (Y in S10), the adjuster 56 adjusts theperiod of time for derivation based on the variance value of the outputvalue of the triaxial gyro sensor 16 or the variance value of the outputvalue of the triaxial acceleration sensor 10 (S12). When the speed isnot smaller than the threshold value (N in S10), step 12 is skipped.

According to this embodiment, the period of time for derivation of theinitial attitude is adjusted based on the variance value of the outputvalue of the triaxial acceleration sensor 10 when the speed of thevehicle 200 is lower than the threshold value. Therefore, the impactfrom vibration of the vehicle 200 is reduced. Further, the period oftime for derivation of the initial attitude is adjusted based on thevariance value of the output value of the triaxial gyro sensor 16 whenthe speed of the vehicle 200 is lower than the threshold value.Therefore, the impact from vibration of the vehicle 200 is reduced.Further, since the impact from vibration of the vehicle 200 is reduced,the accuracy of deriving the attitude is inhibited from becoming poorwhen the triaxial gyro sensor 16 is used. Since the accuracy of derivingthe attitude is inhibited from becoming poor when the triaxial gyrosensor 16 is used, the accuracy of deriving the angular speed isinhibited from becoming poor when the triaxial gyro sensor 16 is used.

Further, since it is ensured that, when the speed of the vehicle 200 islower than the threshold value, the period of time for derivation isextended as the variance value of the output value of the triaxial gyrosensor 16 becomes large, the impact from a noise component is reduced.Since the impact from a noise component is reduced, the accuracy ofderiving the attitude is inhibited from becoming poor. Further, since itis ensured that, when the speed of the vehicle 200 is lower than thethreshold value, the period of time for derivation is extended as thevariance value of the output value of the triaxial acceleration sensor10 becomes large, the accuracy of deriving the attitude is inhibitedfrom becoming poor even if the attitude of the vehicle 200 changes.

A description will now be given of Embodiment 2. Like Embodiment 1,Embodiment 2 relates to an angular speed derivation device configured toderive an angular speed by using a triaxial gyro sensor. The embodimentis directed to the purpose of reducing errors in the attitude anglecollected as the attitude is updated and inhibiting the accuracy ofderiving the yaw angular speed from becoming poor due to the errors.When the initial attitude calculated from the output value of thetriaxial acceleration sensor is updated by the output value of thetriaxial gyro sensor, errors in the offset and sensitivity of thetriaxial gyro sensor used in the calculation are collected as the timeelapses. Accordingly, the accuracy of attitude becomes poor so that theaccuracy of angular speed calculated from the difference thereof alsobecomes poor. And, derivation of the angular speed from the output valueof a monoaxial gyro sensor is affected by the angle of inclination ofthe detection axis caused by a change in the attitude of the vehicle sothat the accuracy becomes poor.

In the angular speed derivation device according to the embodiment, theaccuracy of deriving the angular speed is improved by selecting one ofthe two processes for deriving an angular speed that has a higheraccuracy and performing the selected process. More specifically, a valueof integral of absolute values of angular speed derived by using theoutput value of the triaxial gyro sensor is derived, starting at a pointof time when the initial attitude is derived from the output value ofthe triaxial acceleration sensor. When the value of integral becomesequal to or larger than a threshold value, the angular speed derivationdevice outputs an angular speed derived from the output value of themonoaxial gyro sensor. A description will be given of the differencefrom the foregoing embodiment.

FIG. 4 shows a configuration of the angular speed derivation device 100according to Embodiment 2. The angular speed derivation device 100includes a triaxial acceleration sensor 10, an initial attitudederivation unit 12, a first converter 14, a triaxial gyro sensor 16, anupdating unit 18, a second converter 20, an angular speed derivationunit 22, an output unit 24, a speed sensor 26, a pitch angle derivationunit 30, and a monoaxial angular speed derivation unit 32.

The pitch angle derivation unit 30 receives the output value of thetriaxial acceleration sensor 10 and receives the output value of thespeed sensor 26. The pitch angle derivation unit 30 derives a pitchangle θ of the vehicle 200 based on the received values. FIG. 5 shows asummary of the process in the pitch angle derivation unit 30. Thex-axis, y-axis, and z-axis are defined as described above relative tothe vehicle 200 traveling on a slope so that the triaxial accelerationsensor 10 acquires an acceleration a_(x) in the x-axis direction, anacceleration a_(y) in the y-axis direction, and an acceleration a_(z) inthe z-axis direction. The speed of the vehicle 200 output from the speedsensor 26 is oriented in the direction v. The pitch angle derivationunit 30 acquires an acceleration a by differentiating the speed v. Theacceleration a is oriented in the same direction as the speed v.Denoting the gravitational acceleration by g, the relationship in FIG. 5is given by the following.a _(x) =a−q _(sin) θa _(y)=0a _(z) =q _(cos) θ  (8)

By solving these simultaneous equations, the inclination θ of the slopein FIG. 5 is derived. The inclination θ of the slope represents thepitch angle θ. The pitch angle derivation unit 30 outputs the pitchangle θ to the monoaxial angular speed derivation unit 32.

Of a set of output values of the triaxial gyro sensor 16, the monoaxialangular speed derivation unit 32 acquires the output value for one axis.Further, the monoaxial angular speed derivation unit 32 receives thepitch angle θ from the pitch angle derivation unit 30. The monoaxialangular speed derivation unit 32 derives the angular speed ω as givenbelow.

$\begin{matrix}{\omega = \frac{{Vout} - {Voffset}}{{S \cdot \sin}\;\theta}} & (9)\end{matrix}$

where Vout denotes the output value for one axis, Voffset denotes theoffset value of the triaxial gyro sensor 16, and S (mv/deg/sec) denotesthe sensitivity coefficient of the triaxial gyro sensor 16. Themonoaxial angular speed derivation unit 32 outputs the angular speed coto the output unit 24.

The output unit 24 receives the angular speed derived by the angularspeed derivation unit 22 (hereinafter, also referred to as “triaxialangular speed”). Further, the output unit 24 receives the angular speedderived by the monoaxial angular speed derivation unit 32 (hereinafter,also referred to as “monoaxial angular speed”). Once the initialattitude is derived by the initial attitude derivation unit 12, theoutput unit 24 starts calculating a value of integral of absolute valuesof the triaxial angular speed. When the value of integral is smallerthan a threshold value, the output unit 24 outputs the triaxial angularspeed. And, when the value of integral is equal to or larger than thethreshold value, the output unit 24 outputs the monoaxial angular speedinstead of the triaxial angular speed. When a new initial attitude isderived by the initial attitude derivation unit 12, the output unit 24rests the value of integral.

A description will be given of the operation of the angular speedderivation device 100 having the above configuration. FIG. 6 is aflowchart showing steps for switching by the angular speed derivationdevice 100. The output unit 24 derives a value of integral of absolutevalues of the triaxial angular speed (S30). When the value of integralis equal to or larger than a threshold value (Y in S32), the output unit24 outputs a monoaxial angular speed (S34). When the value of integralis not equal to or larger than the threshold value (N in S32), theoutput unit 24 outputs a triaxial angular speed (S36).

According to this embodiment, an increase in errors integrated in theprocess of updating the attitude is detected by comparing the value ofintegral of absolute values of the triaxial angular speed with thethreshold value. Since the monoaxial angular speed is output instead ofthe triaxial angular speed when the value of integral of absolute valuesof the triaxial angular speed is equal to or larger than the thresholdvalue, the output of triaxial angular speed affected by an error in theattitude is avoided. Since the output of triaxial angular speed affectedby an error in the attitude is avoided, the accuracy of deriving theangular speed output is inhibited from becoming poor. Since themonoaxial angular speed is output, an error in the yaw angular speedcaused by an error in the absolute attitude angle collected in theattitude is reduced.

Embodiment 3

A description will now be given of Embodiment 3. Like the foregoingembodiments, Embodiment 3 relates to an angular speed derivation deviceconfigured to derive an angular speed by using a triaxial gyro sensor.Like Embodiment 2, Embodiment 3 is directed to the purpose of reducingerrors in the attitude angle collected as the attitude is updated andinhibiting the accuracy of deriving the yaw angular speed from becomingpoor due to the errors. In the angular speed derivation device accordingto Embodiment 3, as in the device of Embodiment 2, the accuracy ofderiving the angular speed is improved by selecting one of the twoprocesses for deriving an angular speed that has a higher accuracy andperforming the selected process. More specifically, the initial attitudeis derived from the output value of the triaxial acceleration sensor andthen a variation in the temperature is derived. When the variation inthe temperature becomes equal to or larger than a threshold value, theangular speed derivation device outputs an angular speed derived fromthe output value of the monoaxial gyro sensor. A description will begiven of the difference from the foregoing embodiment.

FIG. 7 shows a configuration of an angular speed derivation device 100according to Embodiment 3. The angular speed derivation device 100includes a triaxial acceleration sensor 10, an initial attitudederivation unit 12, a first converter 14, a triaxial gyro sensor 16, anupdating unit 18, a second converter 20, an angular speed derivationunit 22, an output unit 24, a speed sensor 26, a pitch angle derivationunit 30, a monoaxial angular speed derivation unit 32, a temperaturesensor 34, and an acquisition unit 36.

The temperature sensor 34 is implemented by, for example, asemiconductor temperature sensor and is provided in the triaxial gyrosensor 16 or the neighborhood thereof. In other words, the temperaturesensor 34 detects the temperature of the triaxial gyro sensor 16 or thetemperature around. This is equivalent to detecting the ambienttemperature of the sensor mount in which the triaxial gyro sensor 16 ismounted. The detected temperature is output as, for example, an analogsignal of 0V

5V proportional to the temperature. The acquisition unit 36 receives theanalog signal from the temperature sensor 34. This is equivalent toacquiring the temperature. The acquisition unit 36 is provided with ananalog to digital (AD) converter and subjects the voltage value of thetemperature sensor 34 to AD conversion at a sampling interval of, forexample, 100 msec. The acquisition unit 36 outputs a digital signalresulting from AD conversion to the output unit 24.

The output unit 24 receives the temperature from the acquisition unit36. The output unit 24 also receives the triaxial angular speed from theangular speed derivation unit 22 and receives the monoaxial angularspeed from the monoaxial angular speed derivation unit 32. The outputunit 24 derives the variation in the temperature after the initialattitude is derived by the initial attitude derivation unit 12. Theoutput unit 24 outputs the triaxial angular speed when the variation inthe temperature is smaller than a threshold value. And, the output unit24 outputs the monoaxial angular speed instead of the triaxial angularspeed when the variation in the temperature is equal to or larger thanthe threshold value.

A description will be given of the operation of the angular speedderivation device 100 having the above configuration. FIG. 8 is aflowchart showing steps for switching by the angular speed derivationdevice 100. The acquisition unit 36 acquires the temperature (S50). Whenthe variation in the temperature is equal to or larger than a thresholdvalue (Y in S52), the output unit 24 outputs the monoaxial angular speed(S54). When the variation in the temperature is not equal to or largerthan the threshold value (N in S52), the output unit 24 outputs thetriaxial angular speed (S56).

According to this embodiment, the temperature is compared with athreshold value so that an increase in an error in the gyro sensorcaused by the variation in the environment is detected. Since themonoaxial angular speed is output instead of the triaxial angular speedwhen the temperature is equal to or higher than the threshold value, theoutput of triaxial angular speed affected by an error is avoided. Sincethe output of triaxial angular speed affected by an error is avoided,the accuracy of deriving the angular speed output is inhibited frombecoming poor. Since the monoaxial angular speed is output, an error inthe yaw angular speed caused by an error in the absolute attitude anglecollected in the attitude is reduced.

Embodiment 4

A description will now be given of Embodiment 4. Like the foregoingembodiments, Embodiment 4 relates to an angular speed derivation deviceconfigured to derive an angular speed by using a triaxial gyro sensor.Like Embodiments 2 and 3, Embodiment 4 is directed to the purpose ofreducing errors in the attitude angle collected as the attitude isupdated and inhibiting the accuracy of deriving the yaw angular speedfrom becoming poor due to the errors. In the angular speed derivationdevice according to Embodiment 4, as in the device of Embodiments 2 and3, the accuracy of deriving the angular speed is improved by selectingone of the two processes for deriving an angular speed that has a higheraccuracy and performing the selected process. More specifically, adifference between the pitch angle of the attitude updated by the outputvalue from the triaxial gyro sensor and the pitch angle calculated fromthe triaxial acceleration sensor and the speed sensor is derived. Whenthe difference becomes equal to or larger than a threshold value, theangular speed derivation device outputs an angular speed derived fromthe output value of the monoaxial gyro sensor. A description will begiven of the difference from the foregoing embodiment.

FIG. 9 shows a configuration of an angular speed derivation device 100according to Embodiment 4. The angular speed derivation device 100includes a triaxial acceleration sensor 10, an initial attitudederivation unit 12, a first converter 14, a triaxial gyro sensor 16, anupdating unit 18, a second converter 20, an angular speed derivationunit 22, an output unit 24, a speed sensor 26, a pitch angle derivationunit 30, and a monoaxial angular speed derivation unit 32.

The pitch angle derivation unit 30 outputs the pitch angle θ to themonoaxial angular speed derivation unit 32 and also to the output unit24. Of the attitude in the Euler angle representation, the secondconverter 20 outputs the pitch angle θ to the output unit 24. The outputunit 24 receives the pitch angle θ from the second converter 20(hereinafter, referred to as the “first pitch angle”) and also receivesthe pitch angle θ from the pitch angle derivation unit 30 (hereinafter,referred to as the “second pitch angle”). After the initial attitude isderived by the initial attitude derivation unit 12, the output unit 24derives a difference between the first pitch angle and the second pitchangle. When the difference is smaller than a threshold value, the outputunit 24 outputs the triaxial angular speed. And, when the difference isequal to or larger than the threshold value, the output unit 24 outputsthe monoaxial angular speed instead of the triaxial angular speed.

A description will be given of the operation of the angular speedderivation device 100 having the above configuration. FIG. 10 is aflowchart showing steps for switching by the angular speed derivationdevice 100. The second converter 20 derives the first pitch angle basedon the output value of the triaxial gyro sensor 16 and the output valueof the triaxial acceleration sensor 10 (S70). The pitch angle derivationunit 30 derives the second pitch angle based on the output value of thetriaxial acceleration sensor 10 and the output value of the speed sensor26 (S72). When the difference between the first pitch angle and thesecond pitch angle is equal to or larger than a threshold value (Y inS74), the output unit 24 outputs the monoaxial angular speed (S76). Whenthe difference between the first pitch angle and the second pitch angleis not equal to or larger than a threshold value (Y in S74), the outputunit 24 outputs the triaxial angular speed (S78).

According to this embodiment, a difference between the first pitch angleand the second pitch angle is compared with a threshold value, whereinthe first pitch angle is derived based on the output values of thetriaxial gyro sensor 16 and the triaxial acceleration sensor 10 and thesecond pitch angle is derived based on the output values of the triaxialacceleration sensor 10 and the speed sensor 26. Therefore, an error inthe absolute attitude angle is detected. Since the monoaxial angularspeed is output instead of the triaxial angular speed when thedifference is equal to or higher than the threshold value, the output oftriaxial angular speed affected by an error is avoided. Since the outputof triaxial angular speed affected by an error in the attitude isavoided, the accuracy of deriving the angular speed output is inhibitedfrom becoming poor. Since the monoaxial angular speed is output, theimpact from an error in the absolute attitude angle is reduced.

Described above is an explanation based on an exemplary embodiment. Theembodiment is intended to be illustrative only and it will be understoodby those skilled in the art that various modifications to constitutingelements and processes could be developed and that such modificationsare also within the scope of the present invention.

Arbitrary combinations of Embodiment 1 through Embodiment 4 are alsouseful. According to this variation, the combined benefits ofEmbodiments 1 through 4 are obtained.

What is claimed is:
 1. An angular speed derivation device that can beinstalled in a mobile object, comprising: an initial attitude derivationunit that derives an initial attitude in an Euler angle representationbased on an output value of a triaxial acceleration sensor; a firstconverter that converts the initial attitude in the Euler anglerepresentation derived by the initial attitude derivation unit into aninitial attitude represented by quaternion; an updating unit thatupdates an attitude represented by quaternion by defining the initialattitude represented by quaternion and derived from conversion in thefirst converter as an initial value, and repeatedly solving adifferential equation of the attitude represented by quaternion bysuccessively substituting output values of the triaxial accelerationsensor into the differential equation; a second converter that convertsthe attitude represented by quaternion and updated by the updating unitinto an attitude in the Euler angle representation; an angular speedderivation unit that derives an angular speed based on a time-dependentchange in the attitude in the Euler angle representation derived fromconversion in the second converter; an acquisition unit that acquires aspeed of the mobile object; and a controller that adjusts a period oftime for derivation over which the initial attitude derivation unitaverages the initial attitude when the speed acquired by the acquisitionunit is lower than a threshold value by multiplying a predeterminedangle derivation reference period by the ratio of a current variancevalue of the output value of the triaxial acceleration sensor to avariance value of the output value of the triaxial acceleration sensorobtained in advance.
 2. An angular speed derivation method adapted foran angular speed derivation device that can be installed in a mobileobject, comprising: deriving an initial attitude in an Euler anglerepresentation based on an output value of a triaxial accelerationsensor; converting the initial attitude in the Euler anglerepresentation derived into an initial attitude represented byquaternion; updating the attitude represented by quaternion by definingthe initial attitude represented by quaternion derived from conversionas an initial value, and repeatedly solving a differential equation ofthe attitude represented by quaternion by successively substitutingoutput values of a triaxial gyro sensor into the differential equation;converting the attitude represented by quaternion as updated into anattitude in the Euler angle representation; deriving an angular speedbased on a time-dependent change in the attitude in the Euler anglerepresentation derived from conversion; acquiring a speed of the mobileobject; and adjusting a period of time for derivation over which theinitial attitude in the Euler angle representation is averaged when thespeed acquired is lower than a threshold value by multiplying apredetermined angle derivation reference period by the ratio of acurrent variance value of the output value of the triaxial accelerationsensor to a variance value of the output value of the triaxialacceleration sensor obtained in advance.